
#include <config.h>
#include "lab.hpp"
#include "quat.hpp"
#include "wloader.hpp"
#include "camera.hpp"

#include <GL/gl.h>
#include <GL/glu.h>
#include <SDL/SDL.h>
#include <time.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <fstream>

using namespace std;

void bigdot(Vector v) {
    glBegin(GL_TRIANGLES);
    glVertex3f(v.x-0.03, v.y-0.03, v.z);
    glVertex3f(v.x+0.03, v.y-0.03, v.z);
    glVertex3f(v.x, v.y+0.03, v.z);
    glEnd();
}

void line(Vector &p0, Vector &p1) {
    glBegin(GL_LINES);
    glVertex3f(p0.x, p0.y, p0.z);
    glVertex3f(p1.x, p1.y, p1.z);
    glEnd();
}


VisVector VisVector::from_origin(float r, float g, float b, Vector o, Vector v) {
    VisVector vv;
    vv.r = r;
    vv.g = g;
    vv.b = b;
    vv.p1 = o;
    vv.p2 = o + v;

    return vv;
}

Lab::Lab() : vis_stack(3) {
    ambient.set_color(0.7f, 0.7f, 0.7f);

    for (int i = 0; i < 4; i++) {
        float v = 1.0f;
        lights[i].set_color(v,v,v);
    }
    lights[0].move( 20.0f,  50.0f,  20.0f);
    lights[1].move( 20.0f, -50.0f, -20.0f);
    lights[2].move(-20.0f, -50.0f,  20.0f);
    lights[3].move(-20.0f, -50.0f, -20.0f);
    lights[0].on = true;
    lights[1].on = true;
    lights[2].on = false;
    lights[3].on = false;

    q_sphere = gluNewQuadric();
    gluQuadricDrawStyle(q_sphere, GLU_SILHOUETTE);   // GLU_FILL

    mode = -1;
    paused = false;
    filter = F_ALL; // & ~F_COLLISION_MODELS;
    cam_control_mode = CCM_NONE;

    vis_stack[0].name = "inputs";
    vis_stack[1].name = "forces";
    vis_stack[2].name = "changes";
    vis_focus = 0;

    VisVector vv;
    vv.r = 0.7;
    vv.g = 0.7;
    vv.b = 0.7;
    vis_stack[0].vectors.push_back(vv);
    vis_stack[0].vectors.push_back( VisVector::from_origin( 1, 0, 0, Vector(0,0,0), Vector(0,0,0) ) );
    vis_stack[0].vectors.push_back( VisVector::from_origin( 0, 1, 0, Vector(0,0,0), Vector(0,0,0) ) );
    vis_stack[0].vectors.push_back( VisVector::from_origin( 0, 0, 1, Vector(0,0,0), Vector(0,0,0) ) );
    vis_stack[0].vectors.push_back( VisVector::from_origin( 1, 1, 0, Vector(0,0,0), Vector(0,0,0) ) );
                                    
}


/*******************************************************************************
 *  I N I T
 ******************************************************************************/

void Lab::init() {
    s1 = new Stuff( get_model("test-hammer") );
    s1->move(0, 2, 0);
    s1->prevpos = s1->pos;
    s1->calc_cr();
    //s1->rotvel += Vector(0, 1.0, 0);
    //s1->rotvel = Vector(2.40115, 0.710558, 0.235101);
    //s1->rotvel = Vector(0, 1, 1);
    s1->orientation = Quaternion(0.580443, 0, 0.814301, 0);
    //s1->orientation = Quaternion(M_PI/2.0, Vector::x_axis);
    s1->dyn.mass = 10.0;
    //paused = true;

    s2 = new Stuff( get_model("test-ball") );
    s2->move(2.0, 1, -2);
    //s2->move(2.0, 0, -2);
    s2->prevpos = s2->pos;
    s2->calc_cr();
    s2->dyn.mass = 5.0;
    //s2->velocity.y = 1.0;

    s1->cm = get_cm("test-hammer");
    s2->cm = get_cm("test-ball");

    s1->dyn.from_cm(*s1->cm);
    s2->dyn.from_cm(*s2->cm);
    
    CollisionQuad *cq = new CollisionQuad();
    cq->v[0] = Vector(-2,  2, 3);
    cq->v[1] = Vector( 2,  2, 3);
    cq->v[2] = Vector( 2, -2, 3);
    cq->v[3] = Vector(-2, -2, 3);
    s1->cm->nodes.clear();
    s1->cm->nodes.push_back(cq);

    cq = new CollisionQuad();
    cq->v[0] = Vector(-2,  2, 0);
    cq->v[1] = Vector( 2,  2, 0);
    cq->v[2] = Vector( 2, -2, 0);
    cq->v[3] = Vector(-2, -2, 0);
    s2->cm->nodes.clear();
    s2->cm->nodes.push_back(cq);


    camera = new Camera( Vector(0, 11, 1), Vector(0,2,0) );
    cam_r = 12.0;
    cam_heading = M_PI / 2.0;
    cam_elevation = M_PI / 3.0;
    //cam_heading = M_PI / 2.0;
    //cam_elevation = M_PI / 2 - 0.0001;
    update_cam_pos();

    relight();
}

Lab::~Lab() {
    if (s1) delete s1;
    if (s2) delete s2;
    gluDeleteQuadric(q_sphere);
}

void Lab::update_cam_pos() {
    Vector cam_pos = Vector::from_polar(cam_r, cam_heading, cam_elevation);
    cam_pos += camera->get_focus();
    camera->set_pos( cam_pos );
}


void Lab::on_keydown(SDL_KeyboardEvent *ev) {
    bool ctrl = ev->keysym.mod & KMOD_CTRL;
    bool shift = ev->keysym.mod & KMOD_SHIFT;

    if (!ctrl && !shift) {
        switch (ev->keysym.sym) {
        case SDLK_SPACE: filter ^= F_INTERSECT; break;
        case SDLK_o: filter ^= F_OBJECTS; break;
        case SDLK_e: filter ^= F_COLLISION_MODELS; break;
        case SDLK_r: s2->move(2, 2, 0); s2->velocity = s1->velocity; break;
        case SDLK_UP:  s2->velocity.z -= 1; break;
        case SDLK_DOWN: s2->velocity.z += 1; break;
        case SDLK_RIGHT: s2->velocity.x += 1; break;
        case SDLK_LEFT: s2->velocity.x -= 1; break;
        case SDLK_PAGEUP: s2->velocity.y += 1; break;
        case SDLK_PAGEDOWN: s2->velocity.y -= 1; break;
        case SDLK_b:
            lab_action(1);
            break;
        case SDLK_p: 
            paused = !paused;            
            break;
        case SDLK_n:
            if (++vis_focus >= vis_stack.size()) vis_focus = 0;
            break;
                        
        }
    } else if (!ctrl && shift) {
        switch (ev->keysym.sym) {
        }
    } else if (ctrl && !shift) {
        switch (ev->keysym.sym) {
        }
    }
}

void Lab::on_keyup(SDL_KeyboardEvent *ev) {
    bool ctrl = ev->keysym.mod & KMOD_CTRL;
    //filter = F_ALL;
}

void Lab::on_mouse_down(struct SDL_MouseButtonEvent *ev) {
    if (ev->button == SDL_BUTTON_LEFT) {
        cam_control_mode = CCM_ROTATE;
        SDL_GetRelativeMouseState(NULL, NULL);
    } else if (ev->button == SDL_BUTTON_RIGHT) {
        cam_control_mode = CCM_MOVE;
        SDL_GetRelativeMouseState(NULL, NULL);
    } else if (ev->button == 4) {
        cam_r -= 1;
        update_cam_pos();
    } else if (ev->button == 5) {
        cam_r += 1;
        update_cam_pos();
    }
}

void Lab::on_mouse_up(struct SDL_MouseButtonEvent *ev) {
    cam_control_mode = CCM_NONE;
}

void Lab::lab_action(int action) {
    float _j = 0.5;
    Vector r = Vector(1.0, 0.0, -3.0);
    Vector n(1.0, 0.0, 0.0);
    Vector dw = r.cross(n) * _j;

    s1->rotvel -= dw;
}


static bool test_quad_quad(CollisionQuad &i1, CollisionQuad &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2) {
    //Vector v = s.pos + cq->v[j] * s.orientation;
    return false;
}


/*******************************************************************************
 *  T I C K
 ******************************************************************************/

void Lab::tick(unsigned int ms) {
    static Vector n;
    static Vector p1;
    static Vector p2;

    int x = 0, y = 0;
    Vector d = s2->pos - s1->pos;
    d *= 0.5;
    d += s1->pos;
    camera->set_focus(d);
    update_cam_pos();
    
    if (cam_control_mode == CCM_ROTATE) {
        SDL_GetRelativeMouseState(&x, &y);
        cam_heading += x * M_PI / 270.0;
        cam_elevation -= y * M_PI / 270.0;
        update_cam_pos();
    } else if (cam_control_mode == CCM_MOVE) {
        SDL_GetRelativeMouseState(&x, &y);
        camera->get_focus().x -= 1 * x / 60.0;
        camera->get_focus().z -= 1 * y / 60.0;
        update_cam_pos();
    }

    if (paused) return;

    collision = false;
    
    s1->tick(ms/1000.0);
    s2->tick(ms/1000.0);
    
    /*
    if (s1->cm->test(*s1, *s2, &n, &p1, &p2)) { 
        collision = true;
        
        //s1->dyn.collide(*s1, *s2, n, p1, p2, ms / 1000.0);
    }
    */

    if ( test() ) {
        collision = true;
    }

    //vis_stack[0].vectors[0].p1 = p1;
    //vis_stack[0].vectors[0].p2 = p1 + n*3;

}



static int side(Vector q[], Vector &D, Vector &P) {
    int positive = 0;
    int negative = 0;
    int i;
    float t;
    
    for (i = 0; i < 4; i++) {
        Vector tmp = q[i] - P;
        t = D.dot(tmp);
        if (t < 0) negative++;
        else if (t > 0) positive++;

        if (positive && negative) return 0;
    }

    return (positive ? 1 : -1);
}

bool Lab::test() {
    static Vector v1[4];
    static Vector v2[4];
    static Vector e1a, e1b, e2a, e2b;
    static Vector other[6];
    static Vector n;
    static int i, j;

    CollisionQuad &i1 = (CollisionQuad&)(**s1->cm->nodes.begin());
    CollisionQuad &i2 = (CollisionQuad&)(**s2->cm->nodes.begin());

    for (int j = 0; j < 4; j++) {
        v1[j] = s1->pos + i1.v[j] * s1->orientation;
        v2[j] = s2->pos + i2.v[j] * s2->orientation;
    }

    e1a = v1[1] - v1[0];
    e1b = v1[3] - v1[0];
    e2a = v2[1] - v2[0];
    e2b = v2[3] - v2[0];
    
    face_normal(v1[0], v1[1], v1[2], other[0]);
    face_normal(v1[0], v1[1], v1[2], other[1]);
    other[2] = e1a.cross(e2a);
    other[3] = e1a.cross(e2b);
    //other[4] = e1b.cross(e2a);
    //    other[5] = e1b.cross(e2b);

    int i1min=0, i2min=0, i1max=0, i2max=0 ;
    float pmin = 100000;
    for (i = 0; i < 4; i++) {
        float p1min = 1000000, p1max = -1000000;
        float p2min = 1000000, p2max = -1000000;

        if (other[i].length() == 0) continue;
        other[i].normalize();

        for (j = 0; j < 4; j++) {
    
            float p1 = other[i].dot(v1[j]);
            if (p1 < p1min) { p1min = p1; i1min = j; }
            if (p1 > p1max) { p1max = p1; i1max = j; }

            float p2 = other[i].dot(v2[j]);
            if (p2 < p2min) { p2min = p2; i2min = j; }
            if (p2 > p2max) { p2max = p2; i2max = j; }

        }

        if (p1min > p2max) return false;
        if (p1max < p2min) return false;


        if ((p1max - p2min) < pmin) {
            pmin = p1max - p2min;
            vis_stack[0].vectors[0].p1 = s1->pos;
            vis_stack[0].vectors[0].p2 = v1[i1max];
            vis_stack[0].vectors[3].p1 = s2->pos;
            vis_stack[0].vectors[3].p2 = v2[i2min];
        }
        if ((p2max - p1min) < pmin) {
            pmin = p2max - p1min;
            vis_stack[0].vectors[0].p1 = s1->pos;
            vis_stack[0].vectors[0].p2 = v1[i1min];
            vis_stack[0].vectors[3].p1 = s2->pos;
            vis_stack[0].vectors[3].p2 = v2[i2max];
        }

    }

    return true;
}

/*******************************************************************************
 *  R E N D E R
 ******************************************************************************/


void Lab::render() {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // camera
    glLoadIdentity();
    camera->apply();
    glPushMatrix();

    // lights
    render_light();

    // helpers
    if (filter & F_GRID)
        draw_grid();

    // objects 
    if (filter & F_OBJECTS) {
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_TEXTURE_2D);
        glEnable(GL_LIGHTING);
        glDisable(GL_BLEND);
        
        s1->draw();
        glPopMatrix();
        glPushMatrix();

        s2->draw();
        glPopMatrix();
        glPushMatrix();

        //draw_debug();
    }

    glDisable(GL_TEXTURE_2D);
    glDisable(GL_LIGHTING);
    glEnable(GL_BLEND);

    if (filter & F_COLLISION_MODELS) {
        draw_cm(*s1);
        glPopMatrix();
        glPushMatrix();

        draw_cm(*s2);
        glPopMatrix();
        glPushMatrix();
    }
    

    // Collision Markers
    if (1) {
        glDisable(GL_DEPTH_TEST);
        draw_debug();
    }

    glPopMatrix();
}

void Lab::draw_grid() {
    const int NX = 20;
    const int NZ = 20;
    const float DX = 1.0f;
    const float DZ = 1.0f;

    glEnable(GL_DEPTH_TEST);
    glDisable(GL_TEXTURE_2D);
    glDisable(GL_LIGHTING);
    glColor3f(0, 1.0f, 0);

    glBegin(GL_LINES);
    for (int x = -NX; x <= NX; x++) {
        glVertex3f(x * DX, 0, -NZ * DZ);
        glVertex3f(x * DX, 0,  NZ * DZ);
    }

    for (int z = -NZ; z <= NZ; z++) {
        glVertex3f(-NX * DX, 0, z * DZ);
        glVertex3f( NX * DX, 0, z * DZ);
    }
    glEnd();

    glColor3f(0, 0, 0.05f);
    glBegin(GL_QUADS);
    glVertex3f(-NX * DX, 0, -NZ * DZ);
    glVertex3f(-NX * DX, 0,  NZ * DZ);
    glVertex3f( NX * DX, 0,  NZ * DZ);
    glVertex3f( NX * DX, 0, -NZ * DZ);
    glEnd();
}

void Lab::draw_debug() {
    vector<VisVector>::iterator i;

    for (i = vis_stack[vis_focus].vectors.begin(); 
         i != vis_stack[vis_focus].vectors.end(); 
         i++) {

        glColor3f(i->r, i->g, i->b);
        line(i->p1, i->p2);
    }


    Vector _b = s1->pos + s1->rotvel * 3;
    glColor3f(1, 0, 0);
    line(s1->pos, _b);
}

void Lab::draw_cm(Stuff &s) {
    static GLfloat m[16];
    list<CollisionNode*>::iterator i;

    s.orientation.get_matrix(m);
    if (collision) glColor4f(0.8, 0.0, 0.0, 0.3);
    else glColor4f(0.8, 0.8, 0.8, 0.3);

    for (i = s.cm->nodes.begin(); i != s.cm->nodes.end(); i++) {
        if ((*i)->node_type == CN_SPHERE) {
            CollisionSphere *cs = (CollisionSphere*)*i;
            
            glPushMatrix();
            glTranslatef(s.pos.x, s.pos.y, s.pos.z);
            glMultMatrixf(m);
            glTranslatef(cs->center.x, cs->center.y, cs->center.z);
            gluSphere(q_sphere, cs->radius, 16, 16);
            glPopMatrix();
        }
        else if ((*i)->node_type == CN_QUAD) {
            CollisionQuad *cq = (CollisionQuad*)*i;

            glPushMatrix();

            glBegin(GL_QUADS);
            for (int j = 0; j < 4; j++) {
                Vector v = s.pos + cq->v[j] * s.orientation;
                glVertex3f(v.x, v.y, v.z);
            }
            glEnd();

            glPopMatrix();
        }
    }
}

/* EOF */
